#include "../includes.h"


#define RECOVERY_FIRST_SKIP_TIME 60*10
#define RECOVERY_AVERAGE_TIME 60*12

#define IsJitterFilteringReady() ((MyReference == 2)||(MyReference == 3))

Recovery_	RECOVERY;


int SkipAgingTableCounter;
int recovery_duration;


void Recovery();

void Init_RECOVERY()
{
	RECOVERY.flag = OFF;
	RECOVERY.IntervalCounter = 0;
 	SkipAgingTableCounter = 0;
 	recovery_duration = 0;
}

void Recovery()
{
	double 	cur_offset;

	//sprintf(MESG,"[R]%d,%d,%d,%d,Myref,%d,\r\n",REFERENCE_MODE,RECOVERY.IntervalCounter,RECOVERY.wait_time,RECOVERY.average_time,MyReference);
	//MessageOut(MESG);

	if((REFERENCE_MODE == E1_STATUS_RUN) || (REFERENCE_MODE == E2_STATUS_RUN)){
		RECOVERY.wait_time = 10;
		RECOVERY.average_time = 5;
		if(!IsJitterFilteringReady()){
			//TransmitMessage((BYTE*)"waiting - jitter filtering ready\r\n",PORT_CONTROL);
			MessageOut("waiting - jitter filtering ready...\r\n");
			return;
		}
	}

	CONTROL.HoldoverDuration = 0;
    CONTROL.IntervalCounter = 0;
    
    recovery_duration++;

	//if((!(SYSTEM.TRACK.DOPTYPE&0x01) && ((SYSTEM.TRACK.STATUS>>5)&0x01)) ||
	//	 (recovery_duration > 30*60 && 
	//	 (((SYSTEM.TRACK.STATUS>>5)&0x01) || ((SYSTEM.TRACK.STATUS>>4)&0x01)) || 
	//	 (((SYSTEM.TRACK.STATUS>>3)&0x01) && SYSTEM.POSITION.PositionMode == POSITION_MODE_KNOWN)  ))
	
	//if((!(SYSTEM.TRACK.DOPTYPE&0x01) && ((SYSTEM.TRACK.STATUS>>5)&0x01)))
	if((!(SYSTEM.TRACK.DOPTYPE&0x01) && ((SYSTEM.TRACK.STATUS>>5)&0x01)) ||
		((SYSTEM.POSITION.PositionMode == POSITION_MODE_KNOWN) && ((SYSTEM.TRACK.STATUS>>3)&0x01)) ||
		(recovery_duration > 30*60 && (((SYSTEM.TRACK.STATUS>>5)&0x01) || ((SYSTEM.TRACK.STATUS>>4)&0x01))))
	{
		RECOVERY.IntervalCounter++;
	}
	else{
		RECOVERY.IntervalCounter = 0;
	}

    if (RECOVERY.IntervalCounter == 1) {
	    InitOffsetTable(&TimeOffsetTable);
    	//InitOffsetTable(&TraceOffsetTable);
    }

   	//Modified_RAW_OFFSET = RAW_OFFSET - (SYSTEM.POSITION.CURRENT.HEI-SYSTEM.POSITION.AVERAGE.HEI)/4243.0;
   	Modified_RAW_OFFSET = RAW_OFFSET;

	if ( RECOVERY.IntervalCounter <= RECOVERY.wait_time ) {

		if( RECOVERY.IntervalCounter == RECOVERY.wait_time ) {
			MessageOut("Averaging start...\r\n");
		}
	}

	//------------------------------------------------------------
	// Counter shift
	//------------------------------------------------------------
	else if ( RECOVERY.IntervalCounter <= RECOVERY.wait_time+RECOVERY.average_time ) {

    	InsertTimeOffset(&TimeOffsetTable,(int)Modified_RAW_OFFSET,(int)RAW_DIFFERENCE);
    	//sprintf(MESG,"[[%d,%d]]\r\n",(int)Modified_RAW_OFFSET,(int)RAW_DIFFERENCE);
    	//MessageOut(MESG);

		//CheckReceiverAlarm((int)(Modified_RAW_OFFSET*100.0));

		if ( RECOVERY.IntervalCounter == RECOVERY.wait_time+RECOVERY.average_time ) {


			//##############################################################################
			// 100ns -> Shift
			// 1ms -> Resync
			//##############################################################################
			//if(SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS){

		        if((REFERENCE_MODE == EB_STATUS_RUN) || (REFERENCE_MODE == LMU_STATUS_RUN)){
				//if( SYSTEM.EB.LOCKMODE==EB_LOCKED ){

/*
					if( SYSTEM.EB.out_flag == YES){

						cur_offset = 0;
						Recovery_Offset = 0;

						CONTROL.SYNC = ON;
						SYSTEM.EB.out_flag = OFF;
					}
*/
//					else {

						Recovery_Offset = 0;
						cur_offset = (float)(GetTimeOffset(&TimeOffsetTable,GPS_CUTOFF));

						if(SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS){
							//if( fabs(cur_offset)> 10000 ){
							//if(fabs(cur_offset)> 100){
							if(fabs(cur_offset)> 1000){
							//if( fabs(cur_offset)> 100 || sync_by_ddac == ON){

								CONTROL.SYNC = ON;
								control_sync_type = 2;
								//sync_by_ddac = OFF;
							}
						}
//					}
				}
				else {
					cur_offset = (float)(GetTimeOffset(&TimeOffsetTable,GPS_CUTOFF));
					Recovery_Offset = cur_offset;
				}

			//}

        	if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){
				sprintf(MESG,"Recovery Offset :%.9le, Cur:%.9le\r\n",Recovery_Offset,cur_offset);
				TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    		}
    		
    		LogIt3(LOG_CATA1_NORM, LOG_CATA2_NOTHING, LOG_RECOVERY_END, (int)cur_offset, 0, 0);
		}
	}

	else {

			RECOVERY.flag = OFF;

			RECOVERY.IntervalCounter = 0;
			SkipAgingTableCounter = 50;

       		MeanDAC = CONTROL.RealDAC;

        	InitializeKalmanControl_();
        	
        	freq_stable_cnt=0;
			freq_stable_enough = NO;
	}

   	if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){

		//sprintf(MESG,"%3d:%2d:%2d:%2d [R]%3u> R:%7.2f M:%7.2f RO:%5.2f MRO:%5.2f %RD:%5.2f H:%6ld OT:%5.2f %d,m,%.9le,r_o,%.9le,gCA,%.9le\r\n",
        //  		CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
        //   		RECOVERY.IntervalCounter,CONTROL.RealDAC,MeanDAC,RAW_OFFSET,Modified_RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
        //   		TEMPERATURE.CURRENT,GC.ACCsecond,ModiValue,Recovery_Offset,gCurrentAll);
        sprintf(MESG,"%3d:%2d:%2d:%2d [R]%3u> R:%7.2f M:%7.2f RO:%5.2f MRO:%5.2f RD:%5.2f H:%6ld OT:%5.2f\r\n",
          		CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
           		RECOVERY.IntervalCounter,CONTROL.RealDAC,MeanDAC,RAW_OFFSET,Modified_RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,
           		TEMPERATURE.CURRENT
		);

/*
		sprintf(MESG,"%3d:%2d:%2d:%2d [R]%3u> R:%7.2f M:%7.2f N:%7.2f P:%7.2f C:%7.2f RO:%5.2f MRO:%5.2f %RD:%5.2f H:%6ld C:%4ld AH:%6ld OT:%5.2f %d\r\n",
          		CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
           		RECOVERY.IntervalCounter,CONTROL.RealDAC,MeanDAC,CONTROL.NominalDAC, CONTROL.PrevDAC, CONTROL.ControlDAC,

				RAW_OFFSET,Modified_RAW_OFFSET,RAW_DIFFERENCE,SYSTEM.POSITION.CURRENT.HEI,SYSTEM.POSITION.AveragingCounter,SYSTEM.POSITION.AVERAGE.HEI,
           		TEMPERATURE.CURRENT,GC.ACCsecond);
*/


		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
    }
}


